package rigidbody;

import geometry.Box;
import geometry.TriangleMesh;

import java.util.ArrayList;
import java.util.List;

import javax.vecmath.Color3f;
import javax.vecmath.Matrix4d;
import javax.vecmath.Point3d;
import javax.vecmath.Vector3d;

import collision.Collision;
import collision.OBB;

public class BoxRigidBody extends RigidBody
{
	// Size of the box. All following variables are for collision detection only.
	private double x, y, z;
	private double minDist;
	private Point3d maxPt, minPt;
	private Vector3d tmpNormal = new Vector3d();
	
    public BoxRigidBody(double mass_, Point3d position_, Vector3d dimensions_)
    {
        this(mass_, position_, dimensions_, new Color3f(0.1f, 0.1f, 0.7f));
    }
    
    public BoxRigidBody(double mass_, Point3d position_, Vector3d dimensions_, Color3f color_)
    {
    	super(mass_, position_, new Box(dimensions_.x, dimensions_.y, dimensions_.z, color_));
        
        x = dimensions_.x;
        y = dimensions_.y;
        z = dimensions_.z;
        maxPt = new Point3d( x / 2,  y / 2,  z / 2);
        minPt = new Point3d(-x / 2, -y / 2, -z / 2);
        initASDF();
        m_initialInertia.m00 = y * y + z * z;
        m_initialInertia.m11 = x * x + z * z;
        m_initialInertia.m22 = x * x + y * y;
        m_initialInertia.mul(mass_ / 12);
    }
    
    public boolean detectCollisionUsingOBB(RigidBody b)
    {
		if (b instanceof BoxRigidBody)
		{
			// Detect collision with OBB.
			OBB obb1 = new OBB();
			obb1.center = new Point3d(0, 0, 0);
			obb1.size[0] = x / 2;
			obb1.size[1] = y / 2;
			obb1.size[2] = z / 2;
			obb1.axes[0] = new Vector3d(1, 0, 0);
			obb1.axes[1] = new Vector3d(0, 1, 0);
			obb1.axes[2] = new Vector3d(0, 0, 1);
			
			OBB obb2 = new OBB();
			obb2.center = new Point3d(0, 0, 0);
			obb2.size[0] = ((BoxRigidBody) b).x / 2;
			obb2.size[1] = ((BoxRigidBody) b).y / 2;
			obb2.size[2] = ((BoxRigidBody) b).z / 2;
			obb2.axes[0] = new Vector3d(1, 0, 0);
			obb2.axes[1] = new Vector3d(0, 1, 0);
			obb2.axes[2] = new Vector3d(0, 0, 1);
			
			return OBB.isColliding(obb1, m_transformMatrixInv, obb2, ((BoxRigidBody) b).m_transformMatrix);
		}
		return false;
    }
    
    /**
	 * Check if any point in the other rigid body has hit me, with BoxRigidBody special processed.
	 */
	public List<Collision> detectCollisionWith(RigidBody b)
	{
		List<Collision> coll = new ArrayList<Collision>();
		
		TriangleMesh mesh = b.getTriangleMesh();
		Matrix4d transformMatrixB = b.getTransformMatrix();
		Matrix4d transformMatrix = getTransformMatrix();
		Matrix4d transformMatrixInv = new Matrix4d(transformMatrix);
		transformMatrixInv.invert();
		
		for (Point3d p : mesh._vertices)
		{
		    Point3d ptrans = new Point3d(p);
		    transformMatrixB.transform(ptrans);
		    transformMatrixInv.transform(ptrans);
		    
		    if (-x / 2 < ptrans.x && ptrans.x < x / 2
		    &&  -y / 2 < ptrans.y && ptrans.y < y / 2
		    &&  -z / 2 < ptrans.z && ptrans.z < z / 2)
		    {
		    	getNormal(ptrans);
		    	
		    	transformMatrix.transform(ptrans);
		    	transformMatrix.transform(tmpNormal);
		    	coll.add(new Collision(minDist, b, this, ptrans, new Vector3d(tmpNormal)));
		    }
		}
		
		// Detect edge collisions for two BoxRigidBody to make the result more accurate.
		if (b instanceof BoxRigidBody)
		{
			Point3d pMin = ((BoxRigidBody) b).minPt;
			Point3d pMax = ((BoxRigidBody) b).maxPt;
			Point3d p0 = new Point3d();
			Point3d p1 = new Point3d();
			Point3d ptrans = new Point3d();
			
			// Detect collisions with 12 edges.
			p0.set(pMin.x, pMin.y, pMin.z);
			p1.set(pMax.x, pMin.y, pMin.z);
			transformMatrixB.transform(p0);
			transformMatrixB.transform(p1);
			DetectAndAddEdgeCollision(b, coll, transformMatrix, transformMatrixInv, p0, p1, ptrans);
			
			p0.set(pMin.x, pMin.y, pMin.z);
			p1.set(pMin.x, pMax.y, pMin.z);
			transformMatrixB.transform(p0);
			transformMatrixB.transform(p1);
			DetectAndAddEdgeCollision(b, coll, transformMatrix, transformMatrixInv, p0, p1, ptrans);
			
			p0.set(pMin.x, pMin.y, pMin.z);
			p1.set(pMin.x, pMin.y, pMax.z);
			transformMatrixB.transform(p0);
			transformMatrixB.transform(p1);
			DetectAndAddEdgeCollision(b, coll, transformMatrix, transformMatrixInv, p0, p1, ptrans);
			
			p0.set(pMin.x, pMin.y, pMax.z);
			p1.set(pMin.x, pMax.y, pMax.z);
			transformMatrixB.transform(p0);
			transformMatrixB.transform(p1);
			DetectAndAddEdgeCollision(b, coll, transformMatrix, transformMatrixInv, p0, p1, ptrans);
			
			p0.set(pMin.x, pMin.y, pMax.z);
			p1.set(pMax.x, pMin.y, pMax.z);
			transformMatrixB.transform(p0);
			transformMatrixB.transform(p1);
			DetectAndAddEdgeCollision(b, coll, transformMatrix, transformMatrixInv, p0, p1, ptrans);
			
			p0.set(pMin.x, pMax.y, pMin.z);
			p1.set(pMin.x, pMax.y, pMax.z);
			transformMatrixB.transform(p0);
			transformMatrixB.transform(p1);
			DetectAndAddEdgeCollision(b, coll, transformMatrix, transformMatrixInv, p0, p1, ptrans);
			
			p0.set(pMin.x, pMax.y, pMin.z);
			p1.set(pMax.x, pMax.y, pMin.z);
			transformMatrixB.transform(p0);
			transformMatrixB.transform(p1);
			DetectAndAddEdgeCollision(b, coll, transformMatrix, transformMatrixInv, p0, p1, ptrans);
			
			p0.set(pMax.x, pMin.y, pMin.z);
			p1.set(pMax.x, pMin.y, pMax.z);
			transformMatrixB.transform(p0);
			transformMatrixB.transform(p1);
			DetectAndAddEdgeCollision(b, coll, transformMatrix, transformMatrixInv, p0, p1, ptrans);
			
			p0.set(pMax.x, pMin.y, pMin.z);
			p1.set(pMax.x, pMax.y, pMin.z);
			transformMatrixB.transform(p0);
			transformMatrixB.transform(p1);
			DetectAndAddEdgeCollision(b, coll, transformMatrix, transformMatrixInv, p0, p1, ptrans);
			
			p0.set(pMin.x, pMax.y, pMax.z);
			p1.set(pMax.x, pMax.y, pMax.z);
			transformMatrixB.transform(p0);
			transformMatrixB.transform(p1);
			DetectAndAddEdgeCollision(b, coll, transformMatrix, transformMatrixInv, p0, p1, ptrans);
			
			p0.set(pMax.x, pMin.y, pMax.z);
			p1.set(pMax.x, pMax.y, pMax.z);
			transformMatrixB.transform(p0);
			transformMatrixB.transform(p1);
			DetectAndAddEdgeCollision(b, coll, transformMatrix, transformMatrixInv, p0, p1, ptrans);
			
			p0.set(pMax.x, pMax.y, pMin.z);
			p1.set(pMax.x, pMax.y, pMax.z);
			transformMatrixB.transform(p0);
			transformMatrixB.transform(p1);
			DetectAndAddEdgeCollision(b, coll, transformMatrix, transformMatrixInv, p0, p1, ptrans);
		}
		
		return coll;
	}

	private void DetectAndAddEdgeCollision(RigidBody b, List<Collision> coll,
			Matrix4d transformMatrix, Matrix4d transformMatrixInv, Point3d p0,
			Point3d p1, Point3d ptrans)
	{
		Point3d p;
		p = intersectWithRay(p0, p1, transformMatrixInv);
		if (p != null)
		{
			getNormal(p);
			
			ptrans.set(p);
			transformMatrix.transform(ptrans);
			transformMatrix.transform(tmpNormal);
			coll.add(new Collision(minDist, b, this, ptrans, new Vector3d(tmpNormal)));
		}
	}

	/**
	 * Get the normal for point ptrans, which is in object space.
	 * @param ptrans
	 */
	private void getNormal(Point3d ptrans)
	{
		minDist = Double.MAX_VALUE;
		tryGetNormal(ptrans.x, -x / 2, 1, 0, 0);
		tryGetNormal(ptrans.x, x / 2, -1, 0, 0);
		tryGetNormal(ptrans.y, -y / 2, 0, 1, 0);
		tryGetNormal(ptrans.y, y / 2, 0, -1, 0);
		tryGetNormal(ptrans.z, -z / 2, 0, 0, 1);
		tryGetNormal(ptrans.z, z / 2, 0, 0, -1);
	}
	
	/**
	 * Calculate the intersection points with a ray.
	 * The ray is represented by p = (1 - t)p0 + tp1, where 0 <= t <= 1.
	 * The ray in in world space.
	 * Result point is in object space.
	 */
	public Point3d intersectWithRay(Point3d p0, Point3d p1, Matrix4d transformMatrixInv)
	{
	    Point3d o = new Point3d(p0);
	    transformMatrixInv.transform(o);
	    Vector3d d = new Vector3d();
	    d.sub(p1, p0);
	    transformMatrixInv.transform(d);
	    
	    double ox = o.x;
	    double oy = o.y;
	    double oz = o.z;
	    double dx = d.x;
	    double dy = d.y;
	    double dz = d.z;
	    
	    // a three-slab intersection test. We'll get in and out t values for
	    // all three axes. For instance on the x axis:
	    // o.x + t d.x = 1 => t = (1 - o.x) / d.x
	    // o.x + t d.x = -1 => t = (-1 - o.x) / d.x
	    // This code is straight from Shirley's section 10.9.1

	    double tMin = 0, tMax = 1;

	    double txMin, txMax;
	    if (dx >= 0) {
	      txMin = (minPt.x - ox) / dx;
	      txMax = (maxPt.x - ox) / dx;
	    }
	    else {
	      txMin = (maxPt.x - ox) / dx;
	      txMax = (minPt.x - ox) / dx;
	    }
	    if (tMin > txMax || txMin > tMax)
	      return null;
	    if (txMin > tMin)
	      tMin = txMin;
	    if (txMax < tMax)
	      tMax = txMax;
	  
	    double tyMin, tyMax;
	    if (dy >= 0) {
	      tyMin = (minPt.y - oy) / dy;
	      tyMax = (maxPt.y - oy) / dy;
	    }
	    else {
	      tyMin = (maxPt.y - oy) / dy;
	      tyMax = (minPt.y - oy) / dy;
	    }
	    if (tMin > tyMax || tyMin > tMax)
	      return null;
	    if (tyMin > tMin)
	      tMin = tyMin;
	    if (tyMax < tMax)
	      tMax = tyMax;
	    
	    double tzMin, tzMax;
	    if (dz >= 0) {
	      tzMin = (minPt.z - oz) / dz;
	      tzMax = (maxPt.z - oz) / dz;
	    }
	    else {
	      tzMin = (maxPt.z - oz) / dz;
	      tzMax = (minPt.z - oz) / dz;
	    }
	    if (tMin > tzMax || tzMin > tMax)
	      return null;
	    if (tzMin > tMin)
	      tMin = tzMin;
	    if (tzMax < tMax)
	      tMax = tzMax;
	    
	    // Only consider the case when we have two intersection points.
	    if (tMin < tMax && tMin > 0 && tMax < 1)
	    {
	    	Point3d p = new Point3d();
	    	p.scaleAdd((tMin + tMax) / 2, d, o); // p = o + (tMin + tMax) / 2 * d
	    	return p;
	    }
	    else return null;
	}
	
	private void tryGetNormal(double v, double boundary, double nx, double ny, double nz)
	{
		double d = Math.abs(v - boundary);
		if (d < minDist)
		{
			tmpNormal.x = nx;
			tmpNormal.y = ny;
			tmpNormal.z = nz;
			minDist = d;
		}
	}
}
